-- ball starts at position (0,0)
VAR_XPOS = 0
VAR_YPOS = 0
VAR_XVEL = 0
VAR_YVEL = 0
VAR_THETA = 0b01100
VAR_EPSILON = 0b1100
VAR_THETA_INIT = 0b01111
VAR_EPSILON_INIT = 0b1100
VAR_ACCEL = 0b0000000000000101
VAR_NEGONE = 0b1111111111100000
VAR_VELMULT = 0b1000000
VAR_FLOOR_OFFEST = 0b110000000
VAR_IN0 = 0
VAR_OLDIN0 = 0
VAR_EPSILON_MIN = 0
VAR_EPSILON_MAX = 0b1111
VAR_THETA_MIN = 0
VAR_THETA_MAX = 0b10000
VAR_ONE = 1
VAR_ZERO = 0
VAR_LUT_POS = 500
VAR_X_MAX = 0b101000000000000
VAR_POS_INIT = 0
VAR_LUTMAX = 17
VAR_THETACOS = 0
VAR_EPSILON_LUT = 0
VAR_EPSILON_LUT_OFFSET = 19

--dummy command
CLR A

--startup delay


--initialize stuff
LOAD &VAR_THETA_INIT&,A
STOR A, &VAR_THETA&
--OUT A, 2
LOAD &VAR_EPSILON_INIT&,A
STOR A, &VAR_EPSILON&
--OUT A, 3

--refire from init position
LOAD &VAR_POS_INIT&, A, *refire*
STOR A, &VAR_YPOS&
--get look up table data
--cosine, backwards through sine lut
LOAD &VAR_LUTMAX&, A
LOAD &VAR_THETA&, B
SUB A
STOR A, &VAR_THETACOS&
SINLUT &VAR_THETACOS&, A
STOR A, &VAR_XVEL&
--sinlut
SINLUT &VAR_THETA&, A
STOR A, &VAR_YVEL&

--continue bounce
LOAD &VAR_POS_INIT&, A, *continue_bounce*
STOR A, &VAR_XPOS&

--epsilon
-- LOAD &VAR_EPSILON_LUT_OFFSET&, A
-- LOAD &VAR_EPSILON&, B
-- ADD A
-- STOR A, &VAR_EPSILON_LUT&
-- SINLUT &VAR_EPSILON_LUT&, A

--loop and wait for timer flag
CHKTMR &func_start&, *mainloop*
JMP &mainloop&


--CHECK INPUT BUTTONS
--check for rising edge of bit 0 on inport0
IN 0, A, *func_start*
STOR A, &VAR_IN0&
BRSET A,0,&checkbit0&
JMP &gotobit1&
LOAD &VAR_OLDIN0&, B, *checkbit0*
BRSET B,0,&gotobit1&
--do epsilon minus 1
LOAD &VAR_EPSILON&, A
LOAD &VAR_EPSILON_MIN&, B
--if eplison == epsilon_min go to end, else decrement eplison
CPSE
JMP &decrement_epsilon&
JMP &gotobit1&
LOAD &VAR_ONE&, B, *decrement_epsilon*
LOAD &VAR_EPSILON&, A
SUB A
STOR A, &VAR_EPSILON&
JMP &gotobit1&

--check for rising edge of bit 1 on inport0
--LOAD &VAR_IN0&,A, *gotobit1*
IN 0, A, *gotobit1*
STOR A, &VAR_IN0&
BRSET A,1,&checkbit1&
JMP &gotobit2&
LOAD &VAR_OLDIN0&, B, *checkbit1*
BRSET B,1,&gotobit2&
--do eplison plus 1
LOAD &VAR_EPSILON&, A
LOAD &VAR_EPSILON_MAX&, B
--if eplison == epsilon_max go to end, else increase eplison
CPSE
JMP &increment_epsilon&
JMP &gotobit2&
LOAD &VAR_ONE&, B, *increment_epsilon*
LOAD &VAR_EPSILON&, A
ADD A
STOR A, &VAR_EPSILON&
JMP &gotobit2&

--check for rising edge of bit 2 on inport0
IN 0, A, *gotobit2*
STOR A, &VAR_IN0&
BRSET A,2,&checkbit2&
JMP &gotobit3&
LOAD &VAR_OLDIN0&, B, *checkbit2*
BRSET B,2,&gotobit3&
--do theta minus 1
LOAD &VAR_THETA&, A
LOAD &VAR_THETA_MIN&, B
--if theta == theta_min go to end, else decrement eplison
CPSE
JMP &decrement_theta&
JMP &gotobit3&
LOAD &VAR_ONE&, B, *decrement_theta*
LOAD &VAR_THETA&, A
SUB A
STOR A, &VAR_THETA&
JMP &gotobit3&

--check for rising edge of bit 3 on inport0
--LOAD &VAR_IN0&,A, *gotobit3*
IN 0, A, *gotobit3*
STOR A, &VAR_IN0&
BRSET A,3,&checkbit3&
JMP &gotobit4&
LOAD &VAR_OLDIN0&, B, *checkbit3*
BRSET B,3,&gotobit4&
--do eplison plus 1
LOAD &VAR_THETA&, A
LOAD &VAR_THETA_MAX&, B
--if theta == theta_max go to end, else increase eplison
CPSE
JMP &increment_theta&
JMP &gotobit4&
LOAD &VAR_ONE&, B, *increment_theta*
LOAD &VAR_THETA&, A
ADD A
STOR A, &VAR_THETA&
JMP &gotobit4&

--check for rising edge of bit 4 on inport0
--LOAD &VAR_IN0&,A, *gotobit3*
IN 0, A, *gotobit4*
STOR A, &VAR_IN0&
BRSET A,4,&checkbit4&
JMP &gotobit5&
LOAD &VAR_OLDIN0&, B, *checkbit4*
BRSET B,4,&gotobit5&
JMP &refire&

--update the old inport value
LOAD &VAR_IN0&,A, *gotobit5*
STOR A, &VAR_OLDIN0&

--set theta and epsilon outputs
LOAD &VAR_THETA&,A
OUT A, 2
LOAD &VAR_EPSILON&,A
OUT A, 3

--CALCULATE NEW BALL POSITION
--output positions
LOAD &VAR_XPOS&,A
LOAD &VAR_YPOS&,B
OUT A, 0
OUT B, 1

--calc x pos
LOAD &VAR_XVEL&,A
LOAD &VAR_XPOS&,B
ADD B
STOR B, &VAR_XPOS&

--check if off screen
LOAD &VAR_XPOS&,B
LOAD &VAR_X_MAX&,A
SUB A
--BN &refire&
BN &continue_bounce&
JMP &calcy&

--if new position is negative branch
LOAD &VAR_YVEL&,A,*calcy*
LOAD &VAR_YPOS&,B
ADD B
--LOAD &VAR_FLOOR_OFFEST&, A
--SUB B
BN &bounce&

--calc y velocity
LOAD &VAR_YVEL&,A
LOAD &VAR_ACCEL&,B
SUB A
STOR A, &VAR_YVEL&

--calc y position
LOAD &VAR_YVEL&,A
LOAD &VAR_YPOS&,B
ADD B
STOR B, &VAR_YPOS&

--return to main loop so we can wait for the next timer interrupt
JMP &mainloop&

--invert y velocity
LOAD &VAR_YVEL&,A, *bounce*
NEG A
--LOAD &VAR_NEGONE&,B
--MULT A
STOR A, &VAR_YVEL&

--set eplison
LOAD &VAR_EPSILON&, B
LOAD &VAR_ONE&, A
ADD B
LOAD &VAR_YVEL&,A
MULT A

--help it bounce back
LOAD &VAR_VELMULT&,B
MULT A
STOR A, &VAR_YVEL&

--set y position as 0
STOR 0, &VAR_YPOS&

--return to main loop so we can wait for the next timer interrupt
JMP &mainloop&

